#include "Power.h"
#include "uart.h"

	static uint8_t Msg_pwr[1]; //TODO remove this



void Power_Init(){
	GPIO_InitTypeDef  GPIO_InitStructure;

	//BUZZER
	RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOE|RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOA, ENABLE);
	/* Configure the GPIO_LED pin */
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOE, &GPIO_InitStructure);

	//SYSTEM_POWER 5V
	RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOA, ENABLE);
	/* Configure the GPIO_LED pin */
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);

	//SERVO SUPPLY 6V
	RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOB, ENABLE);
	/* Configure the GPIO_LED pin */
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
}

void Buzzer_on(){
	GPIO_SetBits(GPIOE,GPIO_Pin_12);
}
void Buzzer_off(){
	GPIO_ResetBits(GPIOE,GPIO_Pin_12);
}

void Power_System_on(){
	GPIO_SetBits(GPIOA,GPIO_Pin_4);
}
void Power_System_off(){
	GPIO_ResetBits(GPIOA,GPIO_Pin_4);
}

void Power_Servo_on() {
	GPIO_SetBits(GPIOB, GPIO_Pin_1);

	memset((void*) Msg_pwr, 0, 1);
	Msg_pwr[0] = 1;
	UART_Create_Message(23, Msg_pwr, 1);
}
void Power_Servo_off() {
	GPIO_ResetBits(GPIOB, GPIO_Pin_1);

	memset((void*) Msg_pwr, 0, 1);
	Msg_pwr[0] = 0;
	UART_Create_Message(23, Msg_pwr, 1);
}




